#include "StdAfx.h"
#include "DEEphem.h"


class CHeaderReader
{
public:
	CHeaderReader(ifstream & file): m_file(file) {}

	void Skip(int n)
	{
		m_file.seekg(n);
	}

	template<int N>
	void Buf(int nRead, char (&arr)[N])
	{
		if(N < 1 || nRead < 0 || nRead > N)
		{
			_ASSERT(0);
			return;
		}

		nRead = min(nRead, N);
		m_file.read(arr, nRead);
		nRead = min(nRead, N-1);
		arr[nRead] = 0;
	}

	template<typename T>
	operator T ()
	{
		T val;
		m_file.read((char*)&val, sizeof(val));
		return val;
	}

private:
	ifstream & m_file;
};


CDeEphemeride::CPolynomialCoefficients::CPolynomialCoefficients()
: m_dPrevTime(-1.0) 
{}

const double* CDeEphemeride::CPolynomialCoefficients::GetPolinom(int nSize, double dTime)
{
	if(m_vecData.size() != nSize )
		m_vecData.resize(nSize * 2, 0.0);

	double * pos = &m_vecData[0]; 
	double * vel = &m_vecData[nSize]; 

	if(dTime != m_dPrevTime)
	{
		double dTwoTime = dTime + dTime;

		/* Initialize position polynomial coefficients */

		pos[0] = 1.0;							/* Chebyshev T[0](x) = 1 */
		pos[1] = dTime;							/* Chebyshev T[1](x) = x */
		for (int i = 2; i < nSize; i++) 
		{
			pos[i] = dTwoTime * pos[i-1] - pos[i-2];

			/* Resolve bug with gcc generating -0.0 (also makes
			the smallest represented number equal to zero).	*/
			if (pos[i] * pos[i] == 0.0)
				pos[i] = 0.0;
		}


		/* Initialize derivative polynomial coefficients */

		vel[0] = 0.0;									/* d(1)/dx        = 0  */
		vel[1] = 1.0;									/* d(x)/dx        = 1  */
		vel[2] = dTwoTime + dTwoTime;					/* d(2x^2 - 1)/dx = 4x */
		for (int i=3; i< nSize; i++) 
		{
			vel[i] = dTwoTime * vel[i-1] +  pos[i-1] + pos[i-1] - vel[i-2];
		}
		m_dPrevTime = dTime;
	}

	return pos;
}

CDeEphemeride::CBuffer::CBuffer(ifstream & inFile)
: m_inFile(inFile)
, m_nBlockSize(0)
{
}

void CDeEphemeride::CBuffer::SetCoeffCount(int ncount)
{
	m_nBlockSize = ncount * eBuffAllocCount;
	m_data.resize(m_nBlockSize * arraysize(m_bufs), 0.0);

	for(size_t b = 0; (b < arraysize(m_bufs)); ++b)
		m_bufs[b].pData = &m_data[m_nBlockSize * b];
	m_nBlockSize *= sizeof(double);

	m_inFile.seekg( 0, ios_base::end);
	m_dwFileSize = m_inFile.tellg();
}

const double * CDeEphemeride::CBuffer::GetData(const DWORD dwOffset)
{
	_ASSERT(m_nBlockSize);
	_ASSERT(  dwOffset % (m_nBlockSize / eBuffAllocCount) == 0);
	_ASSERT(!m_data.empty());
	_ASSERT(dwOffset < m_dwFileSize);

	Sbuf * pBuf = NULL;
	for(size_t b = 0; pBuf == NULL && (b < arraysize(m_bufs)); ++b)
	{
		Sbuf & cur = m_bufs[b];
		if( cur.nOrder && dwOffset >= cur.dwFrom && dwOffset < cur.dwTo)
			pBuf = &cur;
	}
	
	if(!pBuf)
	{
		pBuf = (m_bufs[1].nOrder > m_bufs[0].nOrder)? m_bufs: (m_bufs+1);

		int nBlockNum = dwOffset / m_nBlockSize;
		pBuf->dwFrom = nBlockNum * m_nBlockSize;
		pBuf->dwTo = pBuf->dwFrom + m_nBlockSize;
		pBuf->dwTo = min(pBuf->dwTo, m_dwFileSize);
		_ASSERT(dwOffset >= pBuf->dwFrom);
		_ASSERT(dwOffset < pBuf->dwTo);

		m_inFile.seekg(pBuf->dwFrom);
		m_inFile.read((char*)pBuf->pData, pBuf->dwTo - pBuf->dwFrom);
		pBuf->nOrder = m_bufs[(pBuf == m_bufs)? 1: 0].nOrder + 1; 
	}

	const double * dRes = pBuf->pData + ( dwOffset - pBuf->dwFrom ) / sizeof(double);
	return dRes;
}

// ===================================== //

CDeEphemeride::CDeEphemeride (LPCTSTR stzEphemFileName)
: m_buffer(m_inFile)
{
	Zeroize(m_header);

	m_inFile.open(stzEphemFileName, ios::binary);
	if(!m_inFile)
		throw CNoFile(stzEphemFileName);
	CHeaderReader reader(m_inFile);

	/* Read title lines. */
	for(size_t n = 0; n < arraysize(m_header.ttl); ++n)
		reader.Buf(84, m_header.ttl[n]);
 	if(strncmp(m_header.ttl[0], "JPL ", 4) != 0) 
 		THROW_EXCPTN << "wrong file header";


	/* Read constant names. */
	for(size_t n = 0; n < arraysize(m_header.cnam); ++n)
		reader.Buf(6, m_header.cnam[n]);

	/* Read ephemeris start epoch, stop epoch, and step size (in Julian Days). */
	m_header.TimeRange = reader;

	/* Read NCON, AU, EMRAT. */
	m_header.ncon  = reader;
	m_header.au    = reader;
	m_header.emrat = reader;
	m_header.nval  = m_header.ncon;

	/* Read indexes for coefficients in data block.  Written in transposed
	order (Fortran and C matrices are transposed).*/
	for(size_t n = 0; n < arraysize(m_header.IndexPointers); ++n)
		m_header.IndexPointers[n] = reader;

	/* Get ephemeris number */
	m_header.numde = reader;
	m_header.Libration = reader;
	ProcessCoef();

	m_buffer.SetCoeffCount(m_header.ncoeff);

	/* Skip to second block in file. */
	reader.Skip(m_header.ncoeff * 8);

	/* Read ephemeris constants. */
	for(int n = 0; n < m_header.ncon; ++n)
	{
		m_header.cval[n] = reader;
		if (strncmp(m_header.cnam[n], "NCOEFF", 6) == 0) 
		{
			m_header.ncoeff = (int) m_header.cval[n];
			m_header.ksize  = (int) (2 * m_header.cval[n]);
			_ASSERT(m_header.ncoeff == m_header.cval[n]);
			_ASSERT(m_header.ksize  == (2 * m_header.cval[n]));
		}
		else if (strncmp(m_header.cnam[n], "LENUM ", 6) == 0)
			m_header.numle = (int) m_header.cval[n];
		else if (strncmp(m_header.cnam[n], "CLIGHT", 6) == 0)
			m_header.clight = m_header.cval[n];
	}

	if (m_header.numle == 0) 
		m_header.numle = m_header.numde;
}

CDeEphemeride::~CDeEphemeride()
{
}

void CDeEphemeride::ProcessCoef()
{
	/* If there are no coefficients for an IndexPointers[i][] object (i.e., IndexPointers[i].nCoefPerCoord==0),
	then IndexPointers[i].nStartingPoint should contain the value of the next available coefficient
	number rather than 0, as per communication of Myles Standish to Paul Hardy
	on preferred format of ephemeris headers.

	If there are no libration coefficients (i.e., lpt[1]==0), then lpt[0]
	should contain the value of the next available coefficient number rather
	than 0 as well, as per the same communication from Myles Standish. */

	/* First set j to maximum index into IndexPointers[] that has coefficients */
	int j = 0;
	for(size_t n = 0; n < arraysize(m_header.IndexPointers); ++n)
	{
		if (m_header.IndexPointers[n].nCoefPerCoord > 0 && m_header.IndexPointers[n].nStartingPoint > j)
			j = n;
	}

	/* Now set j to next available index count. */
	if (m_header.Libration.nCoefPerCoord > 0 && m_header.Libration.nStartingPoint > j)
		j = m_header.Libration.nCoefPerCoord + m_header.Libration.nCoefPerCoord * m_header.Libration.nSubBlockPerBlock * 3;
	else
		j = m_header.IndexPointers[j].nStartingPoint + m_header.IndexPointers[j].nCoefPerCoord * m_header.IndexPointers[j].nSubBlockPerBlock * (j==11 ? 2 : 3);

	for(size_t n = 0; n < arraysize(m_header.IndexPointers); ++n)
	{
		if (m_header.IndexPointers[n].nStartingPoint == 0) 
			m_header.IndexPointers[n].nStartingPoint = j;
	}
	if (m_header.Libration.nStartingPoint == 0)
		m_header.Libration.nStartingPoint = j;


	/* Set the maximum number of Chebyshev coefficients possible for this file,
	to initialize position and velocity Chebyshev coefficient arrays during
	Chebyshev interpolation. */
	m_header.maxcheby = 0;
	for(size_t n = 0; n < arraysize(m_header.IndexPointers); ++n)
		m_header.maxcheby = max(m_header.IndexPointers[n].nCoefPerCoord, m_header.maxcheby);

	m_header.maxcheby = max(m_header.Libration.nCoefPerCoord, m_header.maxcheby);

	/*	Calculate number of coefficients, starting with
		highest index into a data block (stored in j).	*/

	j = 0;
	for(size_t n = 0; n < arraysize(m_header.IndexPointers); ++n)
	{
		if (m_header.IndexPointers[n].nCoefPerCoord > 0 && m_header.IndexPointers[n].nStartingPoint > m_header.IndexPointers[j].nStartingPoint) 
			j = n;
	}

	/* Now see if the starting point we found is lower than where
	lpt[] starts.  If not, use lpt[] for largest value. */
	if (m_header.Libration.nCoefPerCoord > 0 && m_header.Libration.nStartingPoint > m_header.IndexPointers[j].nStartingPoint) 
	{
		m_header.ncoeff = m_header.Libration.nStartingPoint - 1 +		/* starting point */
			(m_header.Libration.nCoefPerCoord *							/* coefficients per coordinate */
			m_header.Libration.nSubBlockPerBlock) *						/* subblocks per block */
			3;															/* coordinates */
	}
	else 
	{
		m_header.ncoeff = m_header.IndexPointers[j].nStartingPoint - 1 +	/* starting point */
			(m_header.IndexPointers[j].nCoefPerCoord *						/* coefficients per coordinate */
			m_header.IndexPointers[j].nSubBlockPerBlock) *					/* subblocks per block */
			(j == 11 ? 2 : 3);												/* coordinates */
	}

	m_header.ksize = 2* m_header.ncoeff; /* KSIZE = 2*NCOEFF always */

	// printf("Number of coefficients: %d\n", m_header.ncoeff);
}

bool CDeEphemeride::GetCoord(eBodies nBodyNum, double dJD, double dOut[6])
{
	if(nBodyNum < 0 || nBodyNum >= arraysize(m_header.IndexPointers))
		throw CUnknownBoby(nBodyNum);

	ephcom_Header::IndexPointers_t & BodyIndexes = m_header.IndexPointers[nBodyNum];

	if(dJD < m_header.TimeRange.dBegin || dJD > m_header.TimeRange.dEnd)
		throw COutOfTimeRange(dJD);
	double dFromBegin = dJD - m_header.TimeRange.dBegin;
	int nBlockNum = static_cast<int>( dFromBegin / m_header.TimeRange.dStep);

	size_t nfileOffset = (nBlockNum + 2) * m_header.ncoeff * sizeof(double);
	const double * pData = m_buffer.GetData(nfileOffset);
	
	double dBlockTime = dJD - pData[0];													/* Days from block start */
	double dSubIntervalDay = m_header.TimeRange.dStep / BodyIndexes.nSubBlockPerBlock;	/* Days/subinterval */
	int nSubInterval = static_cast<int>(dBlockTime / dSubIntervalDay);
	int nCoordCounts = (nBodyNum == 11) ? 2 : 3;										/* 2 coords for nutation, else 3 */

	size_t nDataOffset = BodyIndexes.nStartingPoint - 1 + nCoordCounts * BodyIndexes.nCoefPerCoord * nSubInterval;
	double dSubTime = dBlockTime - nSubInterval * dSubIntervalDay;

	double dChebyshevTime = dSubTime / dSubIntervalDay;
	dChebyshevTime = (dChebyshevTime + dChebyshevTime) - 1.0;
	if (dChebyshevTime < -1.0 || dChebyshevTime > 1.0)
		THROW_EXCPTN << "Chebyshev time (" << dChebyshevTime << ") is beyond [-1,1] interval";

	GetPosition(dChebyshevTime, dSubIntervalDay, &pData[nDataOffset], nCoordCounts, BodyIndexes.nCoefPerCoord, dOut);

	if (1)  /* Calculate AU, not kilometers */
	{
		double au1 = 1.0 / m_header.au;
		for (size_t j=0; j<6; j++)
			dOut[j] *= au1;
	}
	return true;
}

bool CDeEphemeride::GetCoord1(double dJD, double dOut[12][6])
{
	for(size_t i =0 ; i < 12; i++)
		GetCoord((CDeEphemeride::eBody)i, dJD, dOut[i]);

	for (size_t j=0; j<6; j++)
	{
		dOut[11][j] = dOut[2][j]; /* Move EMBary from Earth spot */
		dOut[2][j] -= dOut[9][j] / (1.0 + m_header.emrat); /* Earth */
		dOut[9][j] += dOut[2][j]; /* Moon (change geo->SS-centric) */
	}
	return true;
}

void CDeEphemeride::GetPosition(double dChebyshevTime, double dSubIntervalDay, const double *dChebyshevCoef, int nCoordCounts, int nCoefPerCoord, double *pv)
{
	const double * pos = m_coeff.GetPolinom(m_header.maxcheby, dChebyshevTime);
	const double * vel = pos + m_header.maxcheby;

	/* Interpolate to get position for each component */
	for (int i=0; i < nCoordCounts; i++)
	{
		pv[i] = 0.0;
		for (int j = nCoefPerCoord-1; j >= 0; j--) 
			pv[i] += pos[j] * dChebyshevCoef[i*nCoefPerCoord + j];
	}

	/* Interpolate velocity (first derivative) */
	for (int i=0; i<nCoordCounts; i++) 
	{
		pv[nCoordCounts + i] = 0.0;
		for (int j = nCoefPerCoord-1; j >= 0; j--) 
			pv[nCoordCounts + i] += vel[j] * dChebyshevCoef[i*nCoefPerCoord + j];
		pv[nCoordCounts + i] *= 2.0 / dSubIntervalDay;
	}
}

